/**
 * ****************************************************************************
 * Project: RoboBridge
 * ----------------------------------------------------------------------------
 * -------------------------- [c]2012 - KrawallMann ---------------------------
 * ---------------------- http://www.krawallmann.de.tc/ -----------------------
 * ****************************************************************************
 * File: RP6Base.java Package:
 * tc.de.krawallmann.robobridge.devices.implementation Version: 1.0 Author(s):
 * KrawallMann
 * ****************************************************************************
 * Description:
 * ****************************************************************************
 */
package tc.de.krawallmann.robobridge.devices.implementation;

import tc.de.krawallmann.robobridge.behaviors.BehaviorCommand;
import tc.de.krawallmann.robobridge.communication.Command;
import tc.de.krawallmann.robobridge.communication.CommandType;
import tc.de.krawallmann.robobridge.devices.AbstractDevice;
import tc.de.krawallmann.robobridge.devices.Robot;
import tc.de.krawallmann.robobridge.util.BitOperations;

/**
 * The Class RP6Base interfaces the RP6's base.
 * 
 * @author KrawallMann
 * @version 1.0
 */
public class RP6Base extends AbstractDevice {

	/** The Constant ACS_LEFT. */
	public static final String ACS_LEFT = "ACS_LEFT";

	/** The Constant ACS_RIGHT. */
	public static final String ACS_RIGHT = "ACS_RIGHT";

	/** The Constant BATTERY. */
	public static final String BATTERY = "BATTERY";

	/** The Constant BUMPERS_LEFT. */
	public static final String BUMPERS_LEFT = "BUMPERS_LEFT";

	/** The Constant BUMPERS_RIGHT. */
	public static final String BUMPERS_RIGHT = "BUMPERS_RIGHT";

	/** The Constant DESIRED_SPEED_LEFT. */
	public static final String DESIRED_SPEED_LEFT = "DESIRED_SPEED_LEFT";

	/** The Constant DESIRED_SPEED_RIGHT. */
	public static final String DESIRED_SPEED_RIGHT = "DESIRED_SPEED_RIGHT";

	/** The Constant DISTANCE_LEFT. */
	public static final String DISTANCE_LEFT = "DISTANCE_LEFT";

	/** The Constant DISTANCE_RIGHT. */
	public static final String DISTANCE_RIGHT = "DISTANCE_RIGHT";

	/** The Constant DRIVE_DISTANCE_COMMAND. */
	public static final int DRIVE_DISTANCE_COMMAND = 3;

	/** The Constant LEDS. */
	public static final String LEDS = "LEDs";

	/** The Constant LIGHT_LEFT. */
	public static final String LIGHT_LEFT = "LIGHT_LEFT";

	/** The Constant LIGHT_RIGHT. */
	public static final String LIGHT_RIGHT = "LIGHT_RIGHT";

	/** The Constant MOTOR_CURRENT_LEFT. */
	public static final String MOTOR_CURRENT_LEFT = "MOTOR_CURRENT_LEFT";

	/** The Constant MOTOR_POWER_LEFT. */
	public static final String MOTOR_POWER_LEFT = "MOTOR_POWER_LEFT";

	/** The Constant MOTOR_POWER_RIGHT. */
	public static final String MOTOR_POWER_RIGHT = "MOTOR_POWER_RIGHT";

	/** The Constant MOVE_AT_SPEED_COMMAND. */
	public static final int MOVE_AT_SPEED_COMMAND = 2;

	/** The Constant MOVEMENT_COMPLETE. */
	public static final String MOVEMENT_COMPLETE = "MOVEMENT_COMPLETE";

	/** The Constant ROTATE_COMMAND. */
	public static final int ROTATE_COMMAND = 4;

	public static final String RP6BASE_LEDS = "RP6BASE_LEDS";

	/** The Constant SPEED_LEFT. */
	public static final String SPEED_LEFT = "SPEED_LEFT";

	/** The Constant SPEED_RIGHT. */
	public static final String SPEED_RIGHT = "SPEED_RIGHT";

	/** The Constant STOP_COMMAND. */
	public static final int STOP_COMMAND = 1;

	/** The Constant MOTOR_CURRENT_RIGHT. */
	private static final String MOTOR_CURRENT_RIGHT = "MOTOR_CURRENT_RIGHT";

	/** The acs left. */
	private transient boolean acsLeft;

	/** The acs right. */
	private transient boolean acsRight;

	/** The bumper left. */
	private transient boolean bumperLeft;

	/** The bumper right. */
	private transient boolean bumperRight;

	/** The desired speed left. */
	private transient int desiredSpeedLeft = 0;

	/** The desired speed right. */
	private transient int desiredSpeedRight = 0;

	/** The dir. */
	private transient Direction dir = Direction.FWD;

	/** The dir command. */
	private transient Command dirCommand;

	/** The distance left. */
	private transient int distanceLeft;

	/** The distance right. */
	private transient int distanceRight;

	/** The has dir command. */
	private transient boolean hasDirCommand;

	/** The has move command. */
	private transient boolean hasMoveCommand;

	/** The leds. */
	private transient int leds = 0;

	/** The light left. */
	private transient int lightLeft = 0;

	/** The light right. */
	private transient int lightRight = 0;

	/** The motor current left. */
	private transient double motorCurrentLeft;

	/** The motor current right. */
	private transient double motorCurrentRight;

	/** The motor power left. */
	private transient double motorPowerLeft;

	/** The motor power right. */
	private transient double motorPowerRight;

	/** The move command. */
	private transient Command moveCommand;

	/** The movement complete. */
	private transient boolean movementComplete = true;

	private BehaviorCommand oldCommand = new BehaviorCommand();

	/** The speed left. */
	private transient int speedLeft;
	/** The speed right. */
	private transient int speedRight;

	/** The u bat. */
	private transient double uBat;

	/**
	 * Instantiates a new r p6 base.
	 * 
	 * @param robot
	 *            the robot
	 * @param deviceId
	 *            the device id
	 */
	public RP6Base(final Robot robot, final int deviceId) {
		super(robot, AbstractDevice.DEVICE_ID_RP6_BASE, deviceId, "Arexx", "RP6 Base");
	}

	/**
	 * Gets the desired speed left.
	 * 
	 * @return the desired speed left
	 */
	public int getDesiredSpeedLeft() {
		return desiredSpeedLeft;
	}

	/**
	 * Gets the desired speed right.
	 * 
	 * @return the desired speed right
	 */
	public int getDesiredSpeedRight() {
		return desiredSpeedRight;
	}

	/**
	 * Gets the direction.
	 * 
	 * @return the direction
	 */
	public Direction getDirection() {
		return dir;
	}

	/**
	 * Gets the distance left.
	 * 
	 * @return the distance left
	 */
	public int getDistanceLeft() {
		return distanceLeft;
	}

	/**
	 * Gets the distance right.
	 * 
	 * @return the distance right
	 */
	public int getDistanceRight() {
		return distanceRight;
	}

	/**
	 * Gets the led.
	 * 
	 * @param led
	 *            the led
	 * @return the led
	 */
	public boolean getLED(final int led) {
		return BitOperations.isBitSet(leds, led);
	}

	/**
	 * Gets the leds.
	 * 
	 * @return the leds
	 */
	public int getLeds() {
		return leds;
	}

	/**
	 * Gets the light left.
	 * 
	 * @return the light left
	 */
	public int getLightLeft() {
		return lightLeft;
	}

	/**
	 * Gets the light right.
	 * 
	 * @return the light right
	 */
	public int getLightRight() {
		return lightRight;
	}

	/**
	 * Gets the motor current left.
	 * 
	 * @return the motor current left
	 */
	public double getMotorCurrentLeft() {
		return motorCurrentLeft;
	}

	/**
	 * Gets the motor current right.
	 * 
	 * @return the motor current right
	 */
	public double getMotorCurrentRight() {
		return motorCurrentRight;
	}

	/**
	 * Gets the motor power left.
	 * 
	 * @return the motor power left
	 */
	public double getMotorPowerLeft() {
		return motorPowerLeft;
	}

	/**
	 * Gets the motor power right.
	 * 
	 * @return the motor power right
	 */
	public double getMotorPowerRight() {
		return motorPowerRight;
	}

	/**
	 * Gets the speed left.
	 * 
	 * @return the speed left
	 */
	public int getSpeedLeft() {
		return speedLeft;
	}

	/**
	 * Gets the speed left cm per second.
	 * 
	 * @return the speed left cm per second
	 */
	public int getSpeedLeftCMPerSecond() {
		return (int) (speedLeft * 0.125);
	}

	/**
	 * Gets the speed right.
	 * 
	 * @return the speed right
	 */
	public int getSpeedRight() {
		return speedRight;
	}

	/**
	 * Gets the speed right cm per second.
	 * 
	 * @return the speed right cm per second
	 */
	public int getSpeedRightCMPerSecond() {
		return (int) (speedRight * 0.125);
	}

	/**
	 * Gets the u bat.
	 * 
	 * @return the u bat
	 */
	public double getuBat() {
		return uBat;
	}

	/**
	 * Checks if is acs left.
	 * 
	 * @return true, if is acs left
	 */
	public boolean isAcsLeft() {
		return acsLeft;
	}

	/**
	 * Checks if is aCS left.
	 * 
	 * @return true, if is aCS left
	 */
	public boolean isACSLeft() {
		return acsLeft;
	}

	/**
	 * Checks if is acs right.
	 * 
	 * @return true, if is acs right
	 */
	public boolean isAcsRight() {
		return acsRight;
	}

	/**
	 * Checks if is aCS right.
	 * 
	 * @return true, if is aCS right
	 */
	public boolean isACSRight() {
		return acsRight;
	}

	/**
	 * Checks if is bumper left.
	 * 
	 * @return true, if is bumper left
	 */
	public boolean isBumperLeft() {
		return bumperLeft;
	}

	/**
	 * Checks if is bumper right.
	 * 
	 * @return true, if is bumper right
	 */
	public boolean isBumperRight() {
		return bumperRight;
	}

	/**
	 * Checks if is movement complete.
	 * 
	 * @return true, if is movement complete
	 */
	public boolean isMovementComplete() {
		return movementComplete;
	}

	/**
	 * Move.
	 * 
	 * @param leftSpeed
	 *            the left speed
	 * @param rightSpeed
	 *            the right speed
	 */
	public void move(final int leftSpeed, final int rightSpeed) {
		final boolean old = movementComplete;
		movementComplete = false;
		moveCommand =
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, getDeviceId(),
						RP6Base.MOVE_AT_SPEED_COMMAND, leftSpeed, rightSpeed);
		hasMoveCommand = true;
		firePropertyChange(RP6Base.MOVEMENT_COMPLETE, old, movementComplete);
	}

	public void moveCommand(final BehaviorCommand cmd) {
		if (cmd.move_value > 0) {
			if (cmd.rotate) {
				rotate(cmd.speed_left, cmd.dir, cmd.move_value);
			} else if (cmd.move) {
				moveDistance(cmd.speed_left, cmd.dir, cmd.move_value);
			}
			cmd.move_value = 0;
		} else if (!(cmd.move || cmd.rotate)) {
			setDirection(cmd.dir);
			move(cmd.speed_left, cmd.speed_right);
		}
	}

	/**
	 * Move distance.
	 * 
	 * @param speed
	 *            the speed
	 * @param dir
	 *            the dir
	 * @param distance
	 *            the distance
	 */
	public void moveDistance(final int speed, final Direction dir, final int distance) {
		movementComplete = false;
		moveCommand =
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, getDeviceId(),
						RP6Base.DRIVE_DISTANCE_COMMAND, speed, dir.getCode(),
						BitOperations.getHighByte(distance), BitOperations.getLowByte(distance));
		hasMoveCommand = true;
		firePropertyChange(RP6Base.MOVEMENT_COMPLETE, true, false);
	}

	/*
	 * (non-Javadoc)
	 * @see
	 * basepackage.IncomingCommandListener#onCommandReceived(basepackage.Command
	 * )
	 */
	@Override
	public boolean onCommandReceived(final Command command) {
		final Integer deviceId = (Integer) command.getParameter(1);
		if (deviceId == null) {
			return false;
		}
		if (containsRelevantData(command, Integer.class, Integer.class)) {
			// 2 ints
			if (deviceId == 3) {
				return updatemovementStatus(command);
			} else if (deviceId == 10) {
				return updateLEDs(command);
			}
		}
		if (containsRelevantData(command, Integer.class, Integer.class, Integer.class)) {
			// 3 ints
			if (deviceId == 1) {
				return updateBumpers(command);
			} else if (deviceId == 2) {
				updateACS(command);
				return true;
			} else if (deviceId == 4) {
				return updateSpeed(command);
			} else if (deviceId == 7) {
				updateDesiredSpeed(command);
				return true;
			} else if (deviceId == 9) {
				updateBattery(command);
				return true;
			}

		}
		if (containsRelevantData(command, Integer.class, Integer.class, Integer.class,
				Integer.class)) {
			// 5 ints
			if ((Integer) command.getParameter(1) == 5) {
				return updateDistance(command);
			} else if ((Integer) command.getParameter(1) == 6) {
				return updateLight(command);
			} else if ((Integer) command.getParameter(1) == 8) {
				return updateMotorCurrent(command);
			}
		}
		return false;
	}

	/**
	 * Rotate.
	 * 
	 * @param speed
	 *            the speed
	 * @param dir
	 *            the dir
	 * @param angle
	 *            the angle
	 */
	public void rotate(final int speed, final Direction dir, final int angle) {
		movementComplete = false;
		if (dir != Direction.RIGHT && dir != Direction.LEFT) {
			throw new IllegalArgumentException("Direction must be left or right!");
		}
		moveCommand =
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, getDeviceId(),
						RP6Base.ROTATE_COMMAND, speed, dir.getCode(),
						BitOperations.getHighByte(angle), BitOperations.getLowByte(angle));
		hasMoveCommand = true;
		firePropertyChange(RP6Base.MOVEMENT_COMPLETE, true, false);
	}

	/**
	 * Sets the direction.
	 * 
	 * @param dir
	 *            the new direction
	 */
	public void setDirection(final Direction dir) {
		final Direction old = this.dir;
		this.dir = dir;
		dirCommand =
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, getDeviceId(), 4, dir.getCode());
		hasDirCommand = true;
		firePropertyChange("DIRECTION", old, dir);
	}

	/**
	 * Sets the leds.
	 * 
	 * @param leds
	 *            the new leds
	 */
	public void setLeds(final int leds) {
		final int old = this.leds;
		this.leds = leds;
		firePropertyChange(RP6Base.RP6BASE_LEDS, old, leds);
		robot.queueOutgoingCommand(new Command(CommandType.UPDATE_ROBOT_DEVICE, false,
				getDeviceId(), 5, leds));
	}

	/**
	 * Stop.
	 */
	public void stop() {
		movementComplete = false;
		moveCommand =
				new Command(CommandType.UPDATE_ROBOT_DEVICE, false, getDeviceId(),
						RP6Base.STOP_COMMAND);
		hasMoveCommand = true;
	}

	/*
	 * (non-Javadoc)
	 * @see tc.de.krawallmann.robobridge.devices.AbstractDevice#toString()
	 */
	@Override
	public String toString() {
		return "RP6Base [deviceId=" + deviceId + ", manufacturer=" + manufacturer + ", model="
				+ model + "]";
	}

	/*
	 * (non-Javadoc)
	 * @see tc.de.krawallmann.robobridge.devices.AbstractDevice#update()
	 */
	@Override
	public void update() {
		super.update();
		if (hasDirCommand) {
			getRobot().queueOutgoingCommand(dirCommand);
			hasDirCommand = false;
		}
		if (hasMoveCommand) {
			getRobot().queueOutgoingCommand(moveCommand);
			hasMoveCommand = false;
		}
	}

	/**
	 * Recalculate motor power.
	 */
	protected void recalculateMotorPower() {
		final double oldLeft = motorPowerLeft;
		final double olRight = motorPowerRight;
		motorPowerLeft = uBat * motorCurrentLeft;
		motorPowerRight = uBat * motorCurrentRight;

		motorPowerLeft = Math.round(motorPowerLeft * 100.) / 100.;
		motorPowerRight = Math.round(motorPowerRight * 100.) / 100.;
		firePropertyChange(RP6Base.MOTOR_POWER_LEFT, oldLeft, motorPowerLeft);
		firePropertyChange(RP6Base.MOTOR_POWER_RIGHT, olRight, motorPowerRight);
	}

	/**
	 * Update acs.
	 * 
	 * @param command
	 *            the command
	 */
	protected void updateACS(final Command command) {
		final boolean oldLeft = acsLeft;
		final boolean oldRight = acsRight;
		final Integer left = command.getParameter(2);
		final Integer right = command.getParameter(3);
		acsLeft = left != 0;
		acsRight = right != 0;
		firePropertyChange(RP6Base.ACS_LEFT, oldLeft, acsLeft);
		firePropertyChange(RP6Base.ACS_RIGHT, oldRight, acsRight);
	}

	/**
	 * Update battery.
	 * 
	 * @param command
	 *            the command
	 */
	protected void updateBattery(final Command command) {
		// Battery
		final double oldBat = uBat;
		uBat =
				BitOperations.combineHighLow((Integer) command.getParameter(2),
						(Integer) command.getParameter(3));
		uBat = uBat * 10d / 1024d;
		uBat = Math.round(uBat * 100.) / 100.;
		firePropertyChange(RP6Base.BATTERY, oldBat, uBat);
		recalculateMotorPower();
	}

	/**
	 * Update bumpers.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateBumpers(final Command command) {
		final boolean oldLeft = bumperLeft;
		final boolean oldRight = bumperRight;
		final Integer left = command.getParameter(2);
		final Integer right = command.getParameter(3);
		bumperLeft = left != 0;
		bumperRight = right != 0;
		firePropertyChange(RP6Base.BUMPERS_LEFT, oldLeft, bumperLeft);
		firePropertyChange(RP6Base.BUMPERS_RIGHT, oldRight, bumperRight);
		return true;
	}

	/**
	 * Update desired speed.
	 * 
	 * @param command
	 *            the command
	 */
	protected void updateDesiredSpeed(final Command command) {
		// DesSpeed
		final int oldLeft = desiredSpeedLeft;
		final int oldRight = desiredSpeedRight;
		desiredSpeedLeft = (Integer) command.getParameter(2);
		desiredSpeedRight = (Integer) command.getParameter(3);
		firePropertyChange(RP6Base.DESIRED_SPEED_LEFT, oldLeft, desiredSpeedLeft);
		firePropertyChange(RP6Base.DESIRED_SPEED_RIGHT, oldRight, desiredSpeedRight);
	}

	/**
	 * Update distance.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateDistance(final Command command) {
		// Distance!
		final int oldLeft = distanceLeft;
		final int oldRight = distanceRight;
		distanceLeft =
				BitOperations.combineHighLow((Integer) command.getParameter(2),
						(Integer) command.getParameter(3));
		distanceRight =
				BitOperations.combineHighLow((Integer) command.getParameter(4),
						(Integer) command.getParameter(5));
		distanceLeft = (int) (distanceLeft * 0.23);
		distanceRight = (int) (distanceRight * 0.23);
		firePropertyChange(RP6Base.DISTANCE_LEFT, oldLeft, distanceLeft);
		firePropertyChange(RP6Base.DISTANCE_RIGHT, oldRight, distanceRight);
		return true;
	}

	/**
	 * Update le ds.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateLEDs(final Command command) {
		final int oldLEDs = leds;
		leds = (Integer) command.getParameter(2);
		firePropertyChange(RP6Base.LEDS, oldLEDs, leds);
		return true;
	}

	/**
	 * Update light.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateLight(final Command command) {
		// Light!
		final int oldLeft = lightLeft;
		final int oldRight = lightRight;
		lightLeft =
				BitOperations.combineHighLow((Integer) command.getParameter(2),
						(Integer) command.getParameter(3));
		lightRight =
				BitOperations.combineHighLow((Integer) command.getParameter(4),
						(Integer) command.getParameter(5));
		firePropertyChange(RP6Base.LIGHT_LEFT, oldLeft, lightLeft);
		firePropertyChange(RP6Base.LIGHT_RIGHT, oldRight, lightRight);
		return true;
	}

	/**
	 * Update motor current.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateMotorCurrent(final Command command) {
		// Motor current
		final double oldLeft = motorCurrentLeft;
		final double oldRight = motorCurrentRight;
		final float voltageOPLeft =
				BitOperations.combineHighLow((Integer) command.getParameter(2),
						(Integer) command.getParameter(3)) * 4f / 1024f;
		final float voltageOPRight =
				BitOperations.combineHighLow((Integer) command.getParameter(4),
						(Integer) command.getParameter(5)) * 4f / 1024f;
		motorCurrentLeft = voltageOPLeft / ((1 + 100000 / 4700) * 0.1);
		motorCurrentRight = voltageOPRight / ((1 + 100000 / 4700) * 0.1);

		motorCurrentLeft = Math.round(motorCurrentLeft * 100.) / 100.;
		motorCurrentRight = Math.round(motorCurrentRight * 100.) / 100.;
		firePropertyChange(RP6Base.MOTOR_CURRENT_LEFT, oldLeft, motorCurrentLeft);
		firePropertyChange(RP6Base.MOTOR_CURRENT_RIGHT, oldRight, motorCurrentRight);
		recalculateMotorPower();
		return true;
	}

	/**
	 * Updatemovement status.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updatemovementStatus(final Command command) {
		final boolean oldMovementComplete = movementComplete;
		movementComplete = (Integer) command.getParameter(2) != 0;
		firePropertyChange(RP6Base.MOVEMENT_COMPLETE, oldMovementComplete, movementComplete);
		return true;
	}

	/**
	 * Update speed.
	 * 
	 * @param command
	 *            the command
	 * @return true, if successful
	 */
	protected boolean updateSpeed(final Command command) {
		// Speed!
		final int oldLeft = speedLeft;
		final int oldRight = speedRight;
		speedLeft = (Integer) command.getParameter(2);
		speedRight = (Integer) command.getParameter(3);
		firePropertyChange(RP6Base.SPEED_LEFT, oldLeft, speedLeft);
		firePropertyChange(RP6Base.SPEED_RIGHT, oldRight, speedRight);
		return true;
	}
}
